﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using System.Numerics;
using System.Text.RegularExpressions;
using OpenCvSharp;
using OpenCvSharp.XFeatures2D;

namespace FLANN
{
    /// <summary>
    /// https://blog.csdn.net/andylanzhiyong/article/details/84778889
    /// </summary>
    public class  FLANNMatcher
    {
        private Mat _Template;
        private Mat _Image;
        private SURF Detector;
        private int _MinHessian = 1200;
        private KeyPoint[] _KeyPointTemplate;
        private KeyPoint[] _KeyPointImage;
        private Mat _DescriptorTemplate;
        private Mat _DescriptorImage;
            public FLANNMatcher()
        {
            Detector = SURF.Create(_MinHessian);
            _DescriptorImage = new Mat();
            _DescriptorTemplate = new Mat();
        }

        public void SetTemplate(Mat template)
        {
            _Template = template.CvtColor(ColorConversionCodes.BGR2GRAY);
            Detector.DetectAndCompute(_Template,new Mat(),out _KeyPointTemplate,_DescriptorTemplate);
        }
        public void Execute(Mat image)
        {
            _Image = image.CvtColor(ColorConversionCodes.BGR2GRAY);
            Detector.DetectAndCompute(_Image,new Mat(),out _KeyPointImage,_DescriptorImage);

            OpenCvSharp.FlannBasedMatcher matcher = new FlannBasedMatcher();
            DMatch[] matches;
            matches = matcher.Match(_DescriptorTemplate, _DescriptorImage);
            
            // 求距离最近的点
            double min_distance = 500;
            for (int i = 0; i < _DescriptorTemplate.Rows; i++)
            {
                double distance = matches[i].Distance;
                if (distance < min_distance)
                    min_distance = distance;
            }
            Console.Write($"min distance: {0}",min_distance);
            
            // 距离较劲且匹配较好的点
            // DMatch[] good_matches = new DMatch[] { };
            var good_matches = new List<DMatch>();
            // ArrayList<DMatch> good_matches;
            for (int i = 0; i < _DescriptorTemplate.Rows; i++)
            {
                double distance = matches[i].Distance;
                if (distance < Math.Max(3 * min_distance, 0.02))
                    good_matches.Add(matches[i]);
            }
            
            
            Mat matches_image = new Mat();
            Cv2.DrawMatches(_Template,_KeyPointTemplate,_Image,_KeyPointImage,
                good_matches,matches_image, Scalar.All(-1),
                Scalar.All(-1),null,DrawMatchesFlags.NotDrawSinglePoints);
            matches_image = matches_image.Resize(new Size(612, 512));
            Cv2.ImShow("Flann Matching Result", matches_image);
            
            // 特征点变换
            List<Point2d> template_obj = new List<Point2d>();
            List<Point2d> target_obj = new List<Point2d>();
            for (int i = 0; i < good_matches.Count; i++)
            {
                // if(good_matches[i].QueryIdx<_KeyPointTemplate.Length && good_matches[i].TrainIdx<_KeyPointImage.Length)
                {
                    Console.WriteLine(i);
                    template_obj.Add(new Point2d(_KeyPointTemplate[good_matches[i].QueryIdx].Pt.X,
                        _KeyPointTemplate[good_matches[i].QueryIdx].Pt.Y));
                    target_obj.Add(new Point2d(_KeyPointImage[good_matches[i].TrainIdx].Pt.X,
                        _KeyPointImage[good_matches[i].TrainIdx].Pt.Y));
                }
                // else
                // {
                //     Console.WriteLine($"{good_matches[i].QueryIdx} {good_matches[i].TrainIdx}");
                // }
            }
            
            Mat imageBH = Cv2.FindHomography(template_obj, target_obj, HomographyMethods.Ransac);
            Console.WriteLine(imageBH);
            
            Point2f[] template_corners = new Point2f[4];
            Point2f[] target_corners = new Point2f[4];
            
            template_corners[0] = new Point2f(0, 0);
            template_corners[1] = new Point2f(_Template.Cols, 0);
            template_corners[2] = new Point2f(_Template.Cols, _Template.Rows);
            template_corners[3] = new Point2f(0, _Template.Rows);
            target_corners = Cv2.PerspectiveTransform(template_corners, imageBH);
            
            
            // Mat dst = new Mat(new Size(2448,2048),MatType.CV_8UC3);
            Console.WriteLine();
            Console.WriteLine(_Image.Size().Width);
            Console.WriteLine(_Image.Size().Height);
            var dst = image.Clone();
            Cv2.Line(dst,target_corners[0].ToPoint(),target_corners[1].ToPoint(),Scalar.Red,3);
            Cv2.Line(dst,target_corners[1].ToPoint(),target_corners[2].ToPoint(),Scalar.Red,3);
            Cv2.Line(dst,target_corners[2].ToPoint(),target_corners[3].ToPoint(),Scalar.Red,3);
            Cv2.Line(dst,target_corners[3].ToPoint(),target_corners[0].ToPoint(),Scalar.Red,3);
            dst = dst.Resize(new Size(612, 512));
            Cv2.ImShow("Dst",dst);

            Cv2.WaitKey(0);

        }
    }
}  